﻿/*****************************************************************//**
 * \file   HapticDeviceControl.cpp
 * \brief  该文件代码处理触觉控制
 *
 * \author Administrator
 * \date   July 2021
 * \modify by Administrator
 * \date   July 2021

 *********************************************************************/

#include "external/ForceDimension/include/dhdc.h"
#include "HapticDeviceControl.h"
#include <iostream>

 /**
  * \brief   HapticDeviceControl类构造函数.
  *
  */
HapticDeviceControl::HapticDeviceControl()
{
	Haptictype = -1;
	bHapticInitDone = false;
	hapticDeviceRead.open("C:\\20210812\\SurgicalRobotPlatform\\SurgicalRobotPlatform\\experiment_data\\hapticForce.txt", std::ios::out);
}

/**
 * \brief   HapticDeviceControl析构函数.
 *
 */
HapticDeviceControl::~HapticDeviceControl()
{

}

/**
 * \brief   触觉设备初始化.
 *
 */
void HapticDeviceControl::HapticDeviceInitial()
{
	OpenAndInitial();
	bHapticInitDone = true;

	return;
}

/**
 * \brief   simga.7触觉设备开机与初始化.
 *
 */
void HapticDeviceControl::OpenAndInitial()
{
	int devicenum = dhdGetDeviceCount();
	int Haptictype_read;

	if (Haptictype != -1 && devicenum > 1)
	{
		switch (Haptictype)
		{
		case HAPTICTYPE_LEFT:
			if ((deviceID = dhdOpenID(0)) < 0)
			{
				std::cout << "error:" << dhdErrorGetLastStr() << std::endl;
				return;
			}
			Haptictype_read = dhdGetSystemType(deviceID);
			if (Haptictype_read != DHD_DEVICE_SIGMA331_LEFT)
			{
				if (dhdClose(deviceID) < 0)
				{
					std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
					return;
				}
				if ((deviceID = dhdOpenID(1)) < 0) {
					std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
					return;
				}
			}
			break;
		case HAPTICTYPE_RIGHT:
			if ((deviceID = dhdOpenID(0)) < 0)
			{
				std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
				return;
			}
			Haptictype_read = dhdGetSystemType(deviceID);
			if (Haptictype_read != DHD_DEVICE_SIGMA331)
			{
				if (dhdClose(deviceID) < 0)
				{
					std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
					return;
				}
				if ((deviceID = dhdOpenID(1)) < 0)
				{
					std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
					return;
				}
			}
			break;
		default:
			break;
		}

		dhdEnableForce(DHD_ON, deviceID);
	}
	else
	{
		if (dhdOpen() < 0)
		{
			std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
			return;
		}

		dhdEnableForce(DHD_ON);
	}

	return;
}

/**
 * \brief   关闭触觉设备.
 *
 */
void HapticDeviceControl::HapticDeviceClose()
{
	Close();

	bHapticInitDone = false;

	return;
}

/**
 * \brief   simga.7触觉设备关闭.
 *
 */
void HapticDeviceControl::Close()
{
	if (dhdClose(deviceID) < 0)
	{
		std::cout << "error: " << dhdErrorGetLastStr() << std::endl;
	}
	return;
}

/**
 * \brief   设置重力补偿.
 *
 */
void HapticDeviceControl::SetGravityCompensation()
{
	GravityCompensation();

	return;
}

/**
 * \brief   simga.7重力补偿.
 *
 */
void HapticDeviceControl::GravityCompensation()
{
	// apply a null force to put the device in gravity compensation
	dhdEnableForce(DHD_ON, deviceID);
	if (dhdSetForceAndTorqueAndGripperForce(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, deviceID) < DHD_NO_ERROR)
	{
		std::cout << "error: can not set force " << dhdErrorGetLastStr() << std::endl;
	}
	return;
}


/**
 * \brief   实时获取simga.7的位置与姿态.
 *
 * \return  simga.7触觉设备的位置与姿态
 */
Eigen::Matrix4d HapticDeviceControl::GetPositionAndOrientation()
{
	dhdGetPositionAndOrientationFrame(&temp(0, 3), &temp(1, 3), &temp(2, 3), dhd_matrix, deviceID);
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			temp(i, j) = dhd_matrix[i][j];
		}
	}

	temp(3, 0) = 0;
	temp(3, 1) = 0;
	temp(3, 2) = 0;
	temp(3, 3) = 1;

	return temp;
}

/**
 * \brief   实时获取simga.7的夹持角度.
 *
 * \return  simga.7触觉设备的夹持角度
 */
double HapticDeviceControl::GetGripperGap()
{
	dhdGetGripperAngleDeg(&grippergap, deviceID);

	return grippergap;
}

/**
 * \brief   获取Haptic device位姿.
 *
 * \param sHapticRunData  Haptic device 运行状态数据结构体变量引用
 */
void HapticDeviceControl::GetHapticPosture(SRobotHapticDeviceData& sHapticRunData)
{
	//将获取的主手运动数据传到hapticDYparameter中，dhd_PostureMatrix是位姿，chip_grap是夹持
	sHapticRunData.dhd_PostureMatrix = GetPositionAndOrientation();
	sHapticRunData.dChipGrap = GetGripperGap();

	sHapticRunData.dhd_orientation = sHapticRunData.dhd_PostureMatrix.block(0, 0, 3, 3);
	sHapticRunData.position = sHapticRunData.dhd_PostureMatrix.block(0, 3, 3, 1);

	return;
}

/**
 * \brief   设置haptic device 力矩和夹具力.
 *
 * \param force 力
 */
void HapticDeviceControl::SetForceAndTorqueAndGripperForce(Eigen::Vector3d force)
{
	SetForceAndTorqueAndGripperForce(force(0), force(1), force(2), 0.0, 0.0, 0.0, 0.0);

	return;
}

/**
 * \brief   设置simga.7的力与力矩,fx,fy,fz为力,tx,ty,tz为力矩.
 *
 * \param fx     x力
 * \param fy     y力
 * \param fz     z力
 * \param tx     x力矩
 * \param ty     y力矩
 * \param tz     z力矩
 * \param gripperforce  夹具力度
 */
void HapticDeviceControl::SetForceAndTorqueAndGripperForce(double fx, double fy, double fz, double tx, double ty, double tz, double gripperforce)
{
	//Set the desired force and torque vectors to be applied to the device end-effector
	dhdEnableForce(DHD_ON, deviceID);
	if (dhdSetForceAndTorqueAndGripperForce(fx, fy, fz, tx, ty, tz, gripperforce, deviceID) < DHD_NO_ERROR)
	{
		hapticDeviceRead << "false" << " " << fx << " " << fy << " " << fz << " " << tx << " " << ty << " " << tz << " " << gripperforce << "\n";

		std::cout << "error: cannot set force " << dhdErrorGetLastStr() << std::endl;
	}
	else {
		hapticDeviceRead << "true" << " " << fx << " " << fy << " " << fz << " " << tx << " " << ty << " " << tz << " " << gripperforce << "\n";
	}

	return;
}

/**
 * \brief   设置simga.7的力与力矩,fx,fy,fz为力,tx,ty,tz为力矩.
 *
 * \param fx     x力
 * \param fy     y力
 * \param fz     z力
 * \param tx     x力矩
 * \param ty     y力矩
 * \param tz     z力矩
 * \param gripperforce  夹具力度
 */
void HapticDeviceControl::SetForceAndTorque(double fx, double fy, double fz, double tx, double ty, double tz)
{
	//Set the desired force and torque vectors to be applied to the device end-effector
	dhdEnableForce(DHD_ON, deviceID);

	if (dhdSetForceAndTorque(fx, fy, fz, tx, ty, tz) < DHD_NO_ERROR)
	{
		hapticDeviceRead << "false" << " " << fx << " " << fy << " " << fz << " " << tx << " " << ty << " " << tz << " " << "\n";

		std::cout << "error: cannot set force " << dhdErrorGetLastStr() << std::endl;
	}
	else {
		hapticDeviceRead << "true" << " " << fx << " " << fy << " " << fz << " " << tx << " " << ty << " " << tz << " " << "\n";
	}

	return;
}
